#include <wiringPi.h>
#include <softPwm.h>
#include <iostream>
#include <unistd.h>
#include "s3010.h"

using namespace std;

S3010::S3010() {
}

S3010::~S3010() {
    softPwmWrite(POWER, 0);
    softPwmWrite(GND, 0);
    cout << "servo deconstructing" << endl;
    sleep(1);
    cout << "servo deconstructed" << endl;
}

// param > 0 : 右转
// param < 0 : 左转
void S3010 ::pwmSet(int pwm) {
    pwm += Middle;
    // cout << "servo pwm \t" << pwm << endl;

    if (pwm > MAX_SERVO) pwm = MAX_SERVO;
    if (pwm < MIN_SERVO) pwm = MIN_SERVO;
    // softPwmWrite(S3010_PIN, pwm);
    pwmWrite(S3010_PIN, pwm);
}

void S3010 ::init() {
    pinMode(S3010_PIN, PWM_OUTPUT);
    // https://blog.csdn.net/weixin_39681161/article/details/116970993
    pwmSetMode(PWM_MODE_MS);
    // 树莓派默认时钟频率为19.2M
    // 此处设置分频系数， 因此现在是19.2M/192 = 100khz
    pwmSetClock(192);
    // pwmSetRange  //默认范围是1024，所以输出频率为100k/1024 ~= 100hz
    pwmWrite(S3010_PIN, Middle);

    // 电源初始化
    softPwmCreate(POWER, 0, 100);
    softPwmCreate(GND, 0, 100);
    softPwmWrite(POWER, POWER_PWM);
    softPwmWrite(GND, 0);

}
